#include "bga/graphics/CameraTrackball.h"
#include <GL/glew.h>
#include <cmath>
#include <iostream>
#include "bga/math/Matrix.h"

#include <sstream>
#define INTENSITE 1

using namespace std;

namespace bga
{

CameraTrackball::CameraTrackball(const Vector3d& position, const Vector3d& center, const Vector3d& up) : Camera(Vector3d(0,0,position.getNorm()), CameraType::TRACKBALL), center(1.f, center)
{
    initializeQuaternionDirection(position, center, up);
    buildTransformationMatrix();

    #ifdef DEBUG
    cout<<this->position.getX()<<";"<<this->position.getY()<<";"<<this->position.getZ()<<endl;
    #endif
}



CameraTrackball::~CameraTrackball()
{
}

void CameraTrackball::onMouseMoved(int dmx, int dmz)
{
    this->rotateX(-(dmz*M_PI)/180);
    this->rotateY(-(dmx*M_PI)/180);
}

void CameraTrackball::onKeyDown(CameraDirection::Direction key)
{
    switch(key)
    {
        case CameraDirection::UP:
            this->translateZ(-INTENSITE);
            break;

        case CameraDirection::DOWN:
            this->translateZ(+INTENSITE);
            break;
/*
        case CameraDirection::LEFT:
            this->translateX(-INTENSITE);
            break;

        case CameraDirection::RIGHT:
            this->translateX(+INTENSITE);
            break;

        case CameraDirection::FORWARD:
            this->translateY(-INTENSITE);
            break;

        case CameraDirection::BACKWARD:
            this->translateY(+INTENSITE);
            break;*/

        default:
            break;
    }
    #ifdef DEBUG
    cout<<this->position.getX()<<";"<<this->position.getY()<<";"<<this->position.getZ()<<endl;
    #endif
}

void CameraTrackball::translateX( double intensite )
{
    this->direction.normalize();
    this->position = this->position + Vector3d(1.0, 0.0, 0.0)  * intensite;
    this->buildTransformationMatrix();
}

void CameraTrackball::translateY( double intensite )
{
    this->position = this->position + Vector3d(0.0, 1.0, 0.0)  * intensite;
    this->buildTransformationMatrix();
}

void CameraTrackball::translateZ( double intensite )
{
    this->position = this->position + Vector3d(0.0, 0.0, 1.0)  * intensite;
    this->buildTransformationMatrix();
}

void CameraTrackball::rotate( double angle, double axisX, double axisY, double axisZ )
{
    /// Not exact
    rotateX(angle);
    rotateY(angle);
    rotateZ(angle);
}

void CameraTrackball::rotateX( double angle )
{
    Quaternion qr(angle, 1.f, 0.f, 0.f, true);
    this->direction = qr*this->direction;
    this->buildTransformationMatrix();
}

void CameraTrackball::rotateY( double angle )
{
    Quaternion qr(angle, 0.f, 1.f, 0.f, true);
    //this->direction = qr * this->direction;
    this->direction = this->direction*qr;
    this->buildTransformationMatrix();
}

void CameraTrackball::rotateZ( double angle )
{
    Quaternion qr(angle, 0.f, 0.f, 1.f, true);
    this->direction = qr * this->direction;
    this->buildTransformationMatrix();
}

void CameraTrackball::buildTransformationMatrix()
{

    this->direction.normalize();
    double s = this->direction.getS();
    double x = this->direction.getX();
    double y = this->direction.getY();
    double z = this->direction.getZ();

    this->modelViewMatrix(0, 0) = 1 - 2 * y * y - 2 * z * z;
    this->modelViewMatrix(0, 1) = 2 * x * y + 2 * s * z;
    this->modelViewMatrix(0, 2) = 2 * x * z - 2 * s * y;
    this->modelViewMatrix(0, 3) = 0;
    this->modelViewMatrix(1, 0) = 2 * x * y - 2 * s * z;
    this->modelViewMatrix(1, 1) = 1 - 2 * x * x - 2 * z * z;
    this->modelViewMatrix(1, 2) = 2 * y * z + 2 * s * x;
    this->modelViewMatrix(1, 3) = 0;
    this->modelViewMatrix(2, 0) = 2 * x * z + 2 * s * y;
    this->modelViewMatrix(2, 1) = 2 * y * z - 2 * s * x;
    this->modelViewMatrix(2, 2) = 1- 2 * x * x - 2 * y * y;
    this->modelViewMatrix(2, 3) = 0;
    this->modelViewMatrix(3, 0) = 0;
    this->modelViewMatrix(3, 1) = 0.f;
    this->modelViewMatrix(3, 2) = -this->position.getZ();
    this->modelViewMatrix(3, 3) = 1.f;


    this->positionMatrix(0, 0) = 1.f;
    this->positionMatrix(0, 1) = 0.f;
    this->positionMatrix(0, 2) = 0.f;
    this->positionMatrix(0, 3) = 0.f;
    this->positionMatrix(1, 0) = 0.f;
    this->positionMatrix(1, 1) = 1.f;
    this->positionMatrix(1, 2) = 0.f;
    this->positionMatrix(1, 3) = 0.f;
    this->positionMatrix(2, 0) = 0.f;
    this->positionMatrix(2, 1) = 0.f;
    this->positionMatrix(2, 2) = 1.f;
    this->positionMatrix(2, 3) = 0.f;
    this->positionMatrix(3, 0) = -center.getX();
    this->positionMatrix(3, 1) = -center.getY();
    this->positionMatrix(3, 2) = -center.getZ();
    this->positionMatrix(3, 3) = 1.f;

    ///changement de repère 1 (global -> camera)
    this->modelViewMatrix = this->modelViewMatrix * this->positionMatrix;

}

Vector3d CameraTrackball::getOrientation() const
{
    //return this->center.toVector3D() - this->position.toVector3D();//Faux! -10 points
    Vector3d v=this->center.toVector3D()-(this->direction.getConjugation()*this->position*this->direction).toVector3D();
    v.normalize();
    return v;
}

Vector3d CameraTrackball::getPosition() const
{
    //return this->center.toVector3D() - this->position.toVector3D();//Faux! -10 points
    Vector3d v=(this->direction.getConjugation()*this->position*this->direction).toVector3D();
    return v;
}

Vector3d CameraTrackball::getMouseOrientation(double x, double y)
{
    Vector3d v= (this->direction.getConjugation()*(Vector3d(0,0,-this->near)+Vector3d(x,y,0))*this->direction).toVector3D();
    v.normalize();
    return v;
}



}
